function [Qf,wf,data,Qstats,Pfit]=FreqFindQ_new(Qf0,varargin)
% Fits data to a Q curve (forced harmonic oscillator).
% Data files must be semicolon delimited, with a 3-line heading.
% 
% INPUT:    Qf0, initial guess for Quality factor (300?)
%           (optional) A, initial input for noise floor (recommended)
%           (optional) data, data matrix previously imported from FreqFindQ or
%           multiple data matrices organized into a cell array.
%
% OUTPUT:   Qf, Quality factor
%           wf, resonance freq, kHz 
%           Qstats = [A B wf Qf]

nVargin = length(varargin);
if nVargin ~= 0
     A = varargin{1};
end

if nVargin < 2
    
    [afile,path] = uigetfile('.txt','Select file'); %file extension, title of window

    raw = importdata(strcat(path,afile),';',3);              %tab delimited
    data = raw.data;

    %Trim?

    h(1) = plot(data(:,1),data(:,2),'displayname','Data');
    if nVargin == 1
        hold on
        h(2) = plot([min(data(:,1)), max(data(:,1))],...
            [A, A], 'color','r','linestyle','--','displayname','Noise floor, A');
        hold off
        legend(h)
    end
    userq = 'Yes';

    while strcmp(userq,'Yes') %Trim
        userq = questdlg('Trim out a region?');
        if ~strcmp(userq,'Yes')
            break
        end
        disp('Select start and end of trim.')
        ends = ginput(2); %Select start and end
        ind1 = find(data(:,1)>ends(1,1),1); %Find start index
        ind2 = find(data(:,1)<ends(2,1),1,'last'); %Find end index
        
        if ind1 == 1
            data = data(ind2:end,:);
        elseif ind2 == length(data(:,1))
            data = data(1:ind1,:);
        else
            data1 = data(1:ind1,:);
            data2 = data(ind2:end,:);
            data = vertcat(data1,data2);
        end
        
        delete(h(1))
        h(1) = plot(data(:,1),data(:,2),'displayname','Data');  
        if nVargin == 1
            hold on
            h(2) = plot([min(data(:,1)), max(data(:,1))],...
                [A, A], 'color','r','linestyle','--','displayname','Noise floor, A');
            hold off
            legend(h)
        end
    end
    
else
    
    data = varargin{2};
    if iscell(data)             %If a cell, combine cells into a matrix
        dataF = [];
        for jj = 1:numel(data)
            dataF = [dataF; data{jj}];
        end
        data = sortrows(dataF);
    end
    h(1) = plot(data(:,1),data(:,2),'b-');  
    
end

x = data(:,1);
y = data(:,2);

m=[0 0 0 Qf0]; %First guess for params: [A B wf Qf0]

%Use max of peak to make first guesses for wf and B
mxy = find(y==max(y),1);
m(3) = x(mxy);
Pmax = y(mxy);
m(2) = (Pmax-m(1))/m(4)^2; %From: P(wf) = A + B*Qf^2

% Define P(w) and fit to curve

options=optimset('TolFun',1e-12);
if isempty(varargin)   %Fit of 4 parameters
    P = @(x,w) x(1)+x(2).*x(3)^4./... 
        ((w.^2-x(3).^2).^2 + w.^2.*x(3).^2./x(4).^2); %Forced harmonic oscillator equation: P(w) = A + B*wf^4/((w^2-wf^2)^2 + w^2*wf^2/Qf^2)
    mF = lsqcurvefit(P,m,x,y,[],[],options);   %Fit equation to data
else
    m(1) = [];  %Reduce m to three parameters
    P = @(x,w) A + x(1).*x(2)^4./... 
        ((w.^2-x(2).^2).^2 + w.^2.*x(2).^2./x(3).^2);   %A is now defined, not free
    m = lsqcurvefit(P,m,x,y,[],[],options);   %Fit equation to data
    mF = [A m];
end

Qf = mF(4);
wf = mF(3);

%Find Q according to wf/fwhm
indPhf = find(y>(Pmax-A)/2);
fwhm = x(indPhf(end))-x(indPhf(1));
Qfwhm = wf/fwhm;
Qstats = [mF Qfwhm];

% Plot fit
Pfit = P(m,x);
Pfit = [x Pfit];
hold on
h(2) = plot(Pfit(:,1),Pfit(:,2),'k--');
set(h(2),'displayname',sprintf('wf %3.3g, Qf %3.3g',wf,Qf),...
    'linewidth',0.5)
xlabel('Frequency (kHz)')
ylabel('Amplitude (nA)')
legend(h)
hold off
